perm filename TRAJ.SAI[SYS,HE]17 blob
sn#115330 filedate 1974-08-15 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00027 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00004 00002 DEFINE WRITE="DEB_HAND"
C00008 00003 EXTERNAL INTEGER SIMPLE PROCEDURE PSTRING(REFERENCE INTEGER ISTRING S)
C00010 00004 SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZBOOLEAN COMP)
C00014 00005 SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV)
C00017 00006 PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD)
C00019 00007 SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH)
C00023 00008 SIMPLE PROCEDURE FLUSH(BOOLEAN FINISHSAFE REAL ARRAY TS)
C00027 00009 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O REAL TH)
C00028 00010 EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTRINTEGER PERIODREFERENCE REAL BUF)
C00032 00011 IFC WAVE THENC
C00034 00012 SIMPLE MESSAGE PROCEDURE SET_TRANS(STRING SREAL ARRAY T)
C00037 00013 PROCEDURE POLY (REAL ARRAY COEFFS INTEGER #SEGS
C00043 00014 PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5
C00044 00015 INTEGER SIMPLE PROCEDURE KISEKIBEGIN
C00048 00016 PROCEDURE TRAJECTORY(REAL ARRAY T0,TF,TRANS)
C00052 00017 SIMPLE MESSAGE PROCEDURE TO_ARM(REAL ARRAY TREFERENCE INTEGER FLAG)
C00055 00018 SIMPLE MESSAGE PROCEDURE GO_ARM(REAL ARRAY TREFERENCE INTEGER FLAG)
C00059 00019 SIMPLE MESSAGE PROCEDURE DRAW_ARM(REAL ARRAY PROFILEINTEGER STAT)
C00064 00020 SIMPLE MESSAGE PROCEDURE START_TRAJECTORY(STRING FILEINTEGER START)
C00066 00021 SIMPLE MESSAGE PROCEDURE OPEN_HAND(REAL OPENING)
C00070 00022 SIMPLE MESSAGE PROCEDURE CHANGE_ARM(REAL ARRAY DIRREAL DIST
C00073 00023 SIMPLE MESSAGE PROCEDURE SET_ARM(STRING STREAL ARRAY V,WRT)
C00075 00024 SIMPLE MESSAGE PROCEDURE DRIVE_ARM(INTEGER JOINTREAL DEGINTEGER TIMEREFERENCE INTEGER FLAG)
C00079 00025 SIMPLE MESSAGE PROCEDURE TALK_ARMINTERP←TRUE
C00080 00026 SIMPLE MESSAGE PROCEDURE OTHER_GO
C00084 00027 SIMPLE MESSAGE PROCEDURE ARM_ASSIGN(STRING SINTEGER VAL)
C00087 ENDMK
C⊗;
DEFINE WRITE="DEB_HAND";
EXTERNAL INTEGER _SKIP_;
DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
.5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
IFC BLUE THENC
REQUIRE "BLUE.DAT" SOURCE_FILE;
ELSEC
REQUIRE "YELLOW.DAT[SYS,HE]" SOURCE_FILE;
ENDC
SAFE REAL ARRAY U1,T[0:101];
SAFE REAL ARRAY DIAG[1:7];
SAFE REAL ARRAY LAST_PLANNED_TRANS,PARK_TRANS[1:4,1:4];
SAFE REAL ARRAY LAST_PLANNED_ARM[1:6];
SAFE REAL ARRAY ARRIVE_ARM,DEPART_ARM[1:4];
REAL F1_ARM,F2_ARM,OBJECT_MASS,OBJECT_KXX,OBJECT_KYY,OBJECT_KZZ;
INTEGER T1_ARM,T2_ARM,MERGE;
INTEGER BFJ;
DEFINE MAX_SEG="8";
SAFE REAL ARRAY POSITION[1:6,0:MAX_SEG];
SAFE REAL ARRAY KOE[1:6,1:(MAX_SEG*4)+6];
SAFE INTEGER ARRAY PERIOD[1:6,1:MAX_SEG];
SAFE INTEGER ARRAY NS[1:6];
SAFE REAL ARRAY POS[0:MAX_SEG];
SAFE REAL ARRAY SOLN[1:(MAX_SEG*3)+6];
SAFE INTEGER ARRAY TIM[1:MAX_SEG];
SAFE STRING ARRAY BANDS[0:'37];
SAFE INTEGER ARRAY TRACK[0:'40];
INTEGER NEXT_BAND,BAND,BAND_NUMBER;
PRELOAD_WITH 0,'1040,0;
SAFE INTEGER ARRAY GOODIE[1:3];
DEFINE UFBWRT="'707000000000";
BOOLEAN FORCED,WAS_FORCED,FAST;
SAFE REAL ARRAY LAST_ARM[1:6];
SAFE REAL ARRAY LAST_TRANS[1:4,1:4];
SAFE REAL ARRAY QA,D[0:16];
SAFE REAL ARRAY TF[1:6];
SAFE REAL ARRAY LU,NR[1:6,1:6];
REAL DIGITS;
INTEGER LOOP,JOINT,DURATION;
SAFE REAL ARRAY REAL_DATA[1:18];
REQUIRE "MATRIX.REL[SYS,HE]" LOAD_MODULE;
REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
EXTERNAL SIMPLE PROCEDURE MMOVE(REFERENCE REAL R,A);
EXTERNAL SIMPLE PROCEDURE MTIMES(REFERENCE REAL R,A,B);
EXTERNAL REAL SIMPLE PROCEDURE TRACET(REFERENCE REAL X,Y);
EXTERNAL SIMPLE PROCEDURE DECOMPOSE(INTEGER I; REAL ARRAY NR,LU);
EXTERNAL SIMPLE PROCEDURE SOLVE(INTEGER I;REAL ARRAY NR,LU,DTH);
EXTERNAL SIMPLE PROCEDURE IMPROVE(INTEGER I; REAL ARRAY NR,LU,DA,DTH;REFERENCE REAL DIGITS);
INTEGER SIMPLE PROCEDURE SQAR (INTEGER I);
RETURN ((I-1)*17);
EXTERNAL INTEGER SIMPLE PROCEDURE PSTRING(REFERENCE INTEGER I;STRING S);
SIMPLE PROCEDURE COMPLETA (INTEGER I;SAFE REAL ARRAY TH);
BEGIN REAL S,C;
INTEGER J,K;
J←SQAR(I);
K←6*(I-1);
S←SIND(TH[I]);
C←COSD(TH[I]);
A[J]←C;
A[J+1]←APAR[K]*S;
A[J+2]←APAR[K+1]*S;
A[J+3]←APAR[K+2]*C;
A[J+4]←S;
A[J+5]←APAR[K+3]*C;
A[J+6]←APAR[K+4]*C;
A[J+7]←APAR[K+5]*S;
END;
SIMPLE PROCEDURE HANDPOS(SAFE REAL ARRAY TH);
BEGIN INTEGER I;
FOR I←1,2,4,5,6 DO COMPLETA(I,TH);
A[SQAR(3)+11]←TH[3];
A[3]←A[3]+SHOLDER[1];
A[7]←A[7]+SHOLDER[2];
MMOVE(A[0],A[0]);
MMOVE(T[0],A[0]);
UNDERFLOW(14);
FOR I←2 STEP 1 UNTIL 6 DO MTIMES(T[SQAR(I)],T[SQAR(I-1)],A[SQAR(I)]);
UNDERFLOW(0);
END;
SIMPLE PROCEDURE UPDATE_SEG;
BEGIN INTEGER I;
HANDPOS(ARM_VECTOR);
FOR I←3 STEP 1 UNTIL 6 DO
BEGIN
ARRBLT(ARM_LINK[I,1,1],T[SQAR(I)],16);
END;
GRASP←ARM_VECTOR[7];
END;
SIMPLE PROCEDURE NOT_LESS(REFERENCE REAL V;REAL L);
BEGIN
IF V<0.0 ∧ V>-L THEN BEGIN V←0; RETURN END;
IF V≥0.0 ∧ V< L THEN V←0;
END;
SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZ;BOOLEAN COMP);
BEGIN PRELOAD_WITH 3,7,11;
SAFE OWN INTEGER ARRAY KEY[1:6];
PRELOAD_WITH [6] 0.0; SAFE OWN REAL ARRAY DIR[1:6];
INTEGER I,J;
REAL MAX,R;
LABEL OUT;
REDUCE(DX_DY_DZ);
IF DX_DY_DZ[1]=0.0 ∧
DX_DY_DZ[2]=0.0 ∧
DX_DY_DZ[3]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←0.0;
RETURN;
END;
UNDERFLOW(14);
ARRBLT(DIR[1],DX_DY_DZ[1],3);
IF ¬COMP THEN BEGIN
IF ABS(T[85+2])<ABS(T[85+6])
THEN BEGIN KEY[4]←2;
KEY[5]←IF ABS(T[85+6])<ABS(T[85+10]) THEN 6 ELSE 10
END
ELSE BEGIN KEY[4]←6;
KEY[5]←IF ABS(T[85+2])<ABS(T[85+10]) THEN 2 ELSE 10
END;
KEY[6]←IF ABS(T[85+4])>ABS(T[85+0])
THEN IF ABS(T[85+8])>ABS(T[85+4]) THEN 9 ELSE 5
ELSE IF ABS(T[85+8])>ABS(T[85+0]) THEN 9 ELSE 1;
MMOVE(U1[SQAR(6)],A[SQAR(6)]);
FOR I←5 STEP -1 UNTIL 2 DO
BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
END;
A[3]←A[7]←0.0;
MMOVE(A[0],A[0]);
MTIMES(U1[0],A[0],U1[SQAR(2)]);
MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
MTIMES(U1[0],Q[0],U1[0]);
FOR I←1 STEP 1 UNTIL 6 DO
FOR J←1 STEP 1 UNTIL 6 DO
NR[J,I]←U1[SQAR(I)+KEY[J]];
DECOMPOSE(6,NR,LU);
END;
SOLVE(6,LU,DIR,DTH);
IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
MAX←0;
FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DIR[I]))>MAX THEN MAX←R;
FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DTH[I]))>MAX THEN
BEGIN FOR J←I+1 STEP 1 UNTIL 6 DO IF ABS(DTH[I]+DTH[J])/R<0.5 THEN
BEGIN DTH[I]←0;
DTH[J]←0;
GO TO OUT;
END;
END;
OUT: FOR I←1 STEP 1 UNTIL 6 DO BEGIN
DTH[I]←IF I=3 THEN DTH[I] ELSE RAD*DTH[I];
NOT_LESS(DTH[I],@-5);
END;
UNDERFLOW(0);
END;
INTEGER SIMPLE PROCEDURE LIMITS(SAFE REAL ARRAY JOINT);
BEGIN INTEGER I;
FOR I←1 STEP 1 UNTIL 6 DO
IF (STOP[I,1]-JOINT[I])*(JOINT[I]-STOP[I,2])<0.0 THEN RETURN(I);
RETURN (0);
END;
SIMPLE PROCEDURE FAST_WRITE;
IF FAST
THEN BEGIN LABEL OK;
START_CODE LABEL WRT;
HRRZ 1,GOODIE;
HRRZ 2,COEFF;
MOVEM 2,(1);
MOVE 3,BAND_NUMBER;
MOVEI 4,10;
WRT: UFBWRT 3,(1);
SOJGE 4,WRT;
MOVEI 2,'20;
ADDM 2,2(1);
JUMPGE 4,OK;
END;
USERERR(0,1,"TRAJECTORY WRITE ERROR");
OK: END
ELSE ARRYOUT('16,COEFF[0],'1000);
SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV);
BEGIN
STRING S;
INTEGER I,L;
REAL XS,YS;
SAFE OWN REAL ARRAY F,M,P[1:4];
SAFE OWN REAL ARRAY INV[1:4,1:4];
EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY R;REFERENCE REAL A;REAL ARRAY V);
EXTERNAL SIMPLE PROCEDURE INVERT(REFERENCE REAL R,A);
SIMPLE PROCEDURE COLVECT(REAL ARRAY R;INTEGER L,I);
BEGIN INTEGER K;
FOR K←1 STEP 1 UNTIL 3 DO R[K]←A[L+(K-1)*4+(I-1)];
R[4]←1.0;
END;
PUSH_FORMAT(9,1);
ARRBLT(F[1],FV[1],3);
ARRBLT(M[1],FV[4],3);
INVERT(INV[1,1],T[SQAR(6)]);
F[4]←M[4]←0.0;
TRANSFORM(F,INV[1,1],F);
TRANSFORM(M,INV[1,1],M);
A[3]←A[7]←0.0;
FOR L←6 STEP -1 UNTIL 1 DO
BEGIN
F[4]←M[4]←0.0;
TRANSFORM (F,A[SQAR(L)],F);
COLVECT(P,SQAR(L),4);
F[4]←1.0;
CROSS(P,P,F);
TRANSFORM(M,A[SQAR(L)],M);
M[4]←1.0;
PLUS(M,M,P);
REDUCE(M);
TQ[L]←IF L=3 THEN F[3] ELSE M[3];
END;
A[3]←A[3]+SHOLDER[1];
A[7]←A[7]+SHOLDER[2];
S←"JOINT TORQUES";
FOR I←1 STEP 1 UNTIL 6 DO S←S&(CVF(TQ[I]));
IF TYP_HAND THEN OUTSTR(S&CRLF&CRLF);
POP_FORMAT;
END;
PRELOAD_WITH
'400000200000,
'200000040000,
'100000010000,
'40000002000,
'20000000400,
'10000000100;
SAFE OWN INTEGER ARRAY GO_WORD[1:6];
SIMPLE PROCEDURE BITS(REFERENCE INTEGER DATWD;REAL SIGN;REFERENCE INTEGER DAT);
START_CODE
MOVE 3,-3('17);
MOVE 1,(3);
MOVE 2,@-1('17);
TDO 1,2;
TRZ 1,(2);
LSH 2,1;
SKIPGE -2('17);
TRO 1,(2);
MOVEM 1,(3);
END;
PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD);
BEGIN INTEGER NF,I,J,K;
REAL R,MAX;
SAFE OWN INTEGER ARRAY ZF[1:6];
SAFE OWN REAL ARRAY FV,TQ[1:6];
NF←FREE_ARM[0,1];
FOR I←1 STEP 1 UNTIL NF DO
BEGIN
ARRBLT(FV[1],FREE_ARM[I,1],6);
FORCE(TQ,FV);
MAX←0.0;
FOR J←1 STEP 1 UNTIL 6 DO
BEGIN LABEL L1;
IF J=BFJ THEN GO TO L1;
FOR K←1 STEP 1 UNTIL I-1 DO IF ZF[K]=J THEN GO TO L1;
IF (R←ABS(TQ[J]/F0[J]))>MAX THEN
BEGIN
MAX←R;
ZF[I]←J;
END;
L1: END;
BITS(DATWD,TQ[ZF[I]],GO_WORD[ZF[I]]);
END;
END;
SIMPLE INTEGER PROCEDURE RUNTIME(SAFE REAL ARRAY DTH);
BEGIN
INTEGER T,TIME,I;
TIME←0;
FOR I←1 STEP 1 UNTIL 6 DO BEGIN
T←ABS(DTH[I])*TIMFAC[I];
IF T>TIME THEN TIME←T END;
RETURN(TIME+(IF TIME THEN 20 ELSE 0));
END "RUNTIME";
SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH);
BEGIN
LABEL F2;
REAL M,IXX,IYY,IZZ;
SAFE OWN REAL ARRAY JI,TQ[1:6];
EXTERNAL REAL SIMPLE PROCEDURE INNER(REFERENCE REAL A,B);
REAL Z; INTEGER I,DIFF,J,K,L,TJ;
RES[1]←0;ARRBLT(RES[2],RES[1],6);
JI[1]←0;ARRBLT(JI[2],JI[1],5);
WAS_FORCED←FORCED;
FORCED←FALSE;
FORCED←OBJECT_MASS;
M←9.33*OBJECT_MASS+M6;
Z←(M6*Z6)/M;
IXX←I6XX+9.33*OBJECT_MASS*OBJECT_KXX↑2;
IYY←I6YY+9.33*OBJECT_MASS*OBJECT_KYY↑2;
IZZ←I6ZZ+9.33*OBJECT_MASS*OBJECT_KZZ↑2;
I←SQAR(6);
JMAT[I]←0.5*(-IXX+IYY+IZZ);
JMAT[I+5]←0.5*(IXX-IYY+IZZ);
JMAT[I+10]←0.5*(IXX+IYY-IZZ);
JMAT[I+11]←JMAT[I+14]←M*Z;
JMAT[I+15]←M;
COMPLETA(1,TH);
COMPLETA(2,TH);
A[SQAR(3)+11]←TH[3];
COMPLETA(4,TH);
COMPLETA(5,TH);
COMPLETA(6,TH);
UNDERFLOW(14);
MTIMES(U1[0],Q[0],A[0]);
MTIMES(D[0],U1[0],JMAT[0]);
JI[1]←TRACET(D[0],U1[0]);
MMOVE(T[0],A[0]);
for J ← 2 step 1 until 6 do begin
TJ←SQAR(J);
DIFF←IF J=3 THEN 17 ELSE 0;
MTIMES (QA[0], Q[DIFF], A[TJ]);
MTIMES (U1[TJ], T[SQAR(J-1)], QA[0]);
for I← 1 step 1 until J-1 do MTIMES (U1[SQAR(I)],U1[SQAR(I)], A[TJ]);
MTIMES(T[TJ],T[SQAR(J-1)],A[TJ]);
F2: FOR I←1 STEP 1 UNTIL J DO RES[I]←RES[I]
+0.107*INNER(U1[SQAR(I)+8],JMAT[TJ+12]);
For K← 1 step 1 until J do begin
MTIMES (D[0],U1[SQAR(K)],JMAT[TJ]);
JI[K]←JI[K] + TRACET(D[0],U1[SQAR(K)]);
END;
END;
T[88]←T[88]+SX;T[92]←T[92]+SY;
FOR I←1 STEP 1 UNTIL 6 DO IF FORCE_ARM[I] THEN BEGIN
FORCE(TQ,FORCE_ARM);
FORCED←TRUE;
FOR I←1 STEP 1 UNTIL 6 DO RES[I]←RES[I]+TQ[I];
DONE;
END;
FOR I←1 STEP 1 UNTIL 6 DO NOT_LESS(RES[I],1.0@-5);
IFC BLUE THENC
FOR I←1 STEP 1 UNTIL 6 DO RES[I]←(RES[I] LAND '777777000000) LOR (SQRT(JI[I]) LSH -18);
I←'770000252504;
ELSEC
FOR I←1 STEP 1 UNTIL 6 DO RES[I]←(RES[I] LAND '777777000000) LOR (SQRT(JI[I]) LSH -18);
I←'770000252502;
ENDC
IF NNUL THEN I←I+'1000000;
IF ¬(FORCED ∨ WAS_FORCED) THEN I←I+'2000000;
FREE_JOINT(I);
ARRBLT(RES[7],I,1);
UNDERFLOW(0);
END;
SIMPLE PROCEDURE FLUSH(BOOLEAN FINISH;SAFE REAL ARRAY TS);
BEGIN INTEGER N,J,I;
IF ¬FRST_OPEN ∨ FINISH
THEN BEGIN
FOR I←1 STEP 1 UNTIL FREEL
DO FOR J←1 STEP 1 UNTIL PTR3+1
DO IF EQU(REF[J],LABELS[I])
THEN BEGIN
IF (STACK[J] LAND '77000000 ) = '26000000 THEN BEGIN
N←PTRS[I]-J+COEFF[(STACK[J] LAND '777777)+1];
REF[J]←NULL;
IF N+J<1 ∨ N+J>PTR3+1
THEN BEGIN
OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
N←PTR3+1-J END;
COEFF[(STACK[J] LAND '777777) +1]←N;
LABEL_LINE[I]←NULL END ELSE BEGIN
START_CODE
MOVE 1,STACK;
ADD 1,J;
HRRE 1,-1(1);
MOVEM 1,N END;
N←PTRS[I]-J+N;
REF[J]←NULL;
IF N+J<1 ∨ N+J>PTR3+1
THEN BEGIN
OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
N←PTR3+1-J END;
STACK[J]←(N LAND '777777) LOR (STACK[J] LAND '777000000);
LABEL_LINE[I]←NULL END;END;
FOR I←1 STEP 1 UNTIL PTR3
DO IF LENGTH(REF[I])
THEN BEGIN OUTSTR(CODE_LINE[I]&REF[I]&" UNDEFINED"&'15&'12);
STACK[I]←(PTR3+1-I) LOR '102000000;
REF[I]←NULL;
LABEL_LINE[I]←NULL END;
FREEL←0;
FOR I←1 STEP 1 UNTIL PTR3 DO
IF (J←(STACK[I] LAND '777000000))='102000000 ∨ J='16000000 THEN BEGIN
START_CODE
MOVE 1,STACK;
ADD 1,I;
HRRE 1,-1(1);
MOVEM 1,N END;
IF ¬N ∨ N+I<1 ∨ N+I>PTR3+1 THEN BEGIN
OUTSTR(CODE_LINE[I]&"JUMP OUT OF RANGE"&CRLF);
STACK[I]←(PTR3+1-I) LOR '102000000 END;
END;
FOR I←1 STEP 1 UNTIL MAC DO BBEG[I]←LLAB[I]←1;
IF PTR3+PTR4≥PTR2 THEN USERERR(0,1,"TRAJECTORY FILE TOO LONG");
ARRBLT(COEFF[PTR4+1],STACK[1],PTR3);
COEFF[PTR4+PTR3+1]←0;
I←-(PTR4+1+(MASTER LSH 18));
ARRBLT(COEFF[0],I,1);
COEFF['1000]←0;
PTR4←PTR3←0;
PTR2←512;
FAST_WRITE;
END ELSE FRST_OPEN←FALSE;
IF FINISH THEN BEGIN
IF ¬FAST THEN RELEASE('16);
RETURN END;
ARRTRAN(TH,TS);
SCHEINMAN(DIAG,TH);
END;
SIMPLE PROCEDURE FLUSHP(INTEGER N;SAFE REAL ARRAY TH);
BEGIN FRST_OPEN←FALSE;
IF PTR3+PTR4+N≥PTR2 THEN FLUSH(0,TH);
END;
SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
BEGIN
SAFE OWN REAL ARRAY OP,A,T[1:4];
UNIT(O,O);
SCALE(OP,O,DOT(P,O));
DIFFERENCE(A,P,OP);
CROSS(T,O,A);
SCALE(T,T,SIND(TH));
SCALE(P,A,COSD(TH));
PLUS(P,P,T);
PLUS(P,P,OP);
REDUCE(P);
END;
PROCEDURE RESET_CONO;
BEGIN
FREE_ARM[0,1]←FORCE_ARM[1]←OBJECT_MASS←OBJECT_KXX←OBJECT_KYY←OBJECT_KZZ←0;
NNUL←0;
BFJ←0;
ARRBLT(FORCE_ARM[2],FORCE_ARM[1],5);
END;
EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTR;INTEGER PERIOD;REFERENCE REAL BUF);
SIMPLE PROCEDURE PACK_UP;
BEGIN
INTEGER P1,I,J,K;
REAL R1,R2;
SAFE OWN INTEGER ARRAY LOOPP,NXT,ALT[1:6];
SAFE OWN REAL ARRAY BUF[0:4];
COEFF[PTR4←PTR4+1]←DURATION;
STACK[PTR3←PTR3+1]←'21000000 LOR PTR4;
IF MERGE THEN
BEGIN
STACK[PTR3]←STACK[PTR3] LOR '400000000;
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE
END;
FOR JOINT←1 STEP 1 UNTIL 6 DO
BEGIN
ARRBLT(BUF[0],KOE[JOINT,1],5);
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,1],BUF[0]);
NXT[JOINT]←PTR4
END;
COEFF[PTR4←PTR4+1]←PTR2;
FOR JOINT←1 STEP 1 UNTIL 6 DO
BEGIN
BUF[4]←0;
FOR I←2 STEP 1 UNTIL NS[JOINT] DO
BEGIN
ARRBLT(BUF[0],KOE[JOINT,I*4-2],(IF I=NS[JOINT] THEN 5 ELSE 4));
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
NXT[JOINT]←PTR4;
IF LOOP THEN
BEGIN
IF I=3 THEN LOOPP[JOINT]←PTR4;
IF I=6 THEN ALT[JOINT]←PTR4
END;
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7
END;
IF LOOP THEN
BEGIN
BUF[0]←POSITION[JOINT,6];
FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I-2+J];
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
NXT[JOINT]←PTR4;
COEFF[P1←ALT[JOINT]]←(PTR4 LSH 27) LOR COEFF[P1];
IF JOINT=6 THEN COEFF[P1+1]←'100000000000 LOR (LOOP LSH 18) LOR COEFF[P1+1];
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←499;
BUF[0]←POSITION[JOINT,0];
FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I+2+J];
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
COEFF[PTR4]←(LOOPP[JOINT] LSH 18) LOR COEFF[PTR4];
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←473
END
END;
END;
INTEGER SIMPLE PROCEDURE QUADROOT(REAL A0,A1,A2,A3;REFERENCE REAL ANG,T);BEGIN
REAL DISC,B;
INTEGER I;
IF A3 ∧ (DISC←(A2/(3*A3))↑2-A1/(3*A3))≥0 THEN DISC←SQRT(DISC) ELSE RETURN(0);
B←A2/(3*A3);
T←DISC-B;
FOR I←0,1 DO BEGIN
IF 0< T <1.0 THEN
BEGIN
ANG←(((A3*T)+A2)*T+A1)*T+A0;
IF ANG>STOP[JOINT,2] THEN BEGIN ANG←STOP[JOINT,2];RETURN(1) END;
IF ANG<STOP[JOINT,1] THEN BEGIN ANG←STOP[JOINT,1];RETURN(1) END;
END;
T←-(DISC+B);
END;
RETURN(0);
END;
IFC WAVE THENC
SIMPLE PROCEDURE STEP_ARM(INTEGER JOINT;REAL DTH;INTEGER TIME);
BEGIN
INTEGER I;
IF ARM_EXECUTE THEN RETURN;
FLUSHP(100,LAST_ARM);
FOR I←1 STEP 1 UNTIL 6 DO
BEGIN
POSITION[I,0]←LAST_ARM[I];
NS[I]←2;
PERIOD[I,1]←10;
PERIOD[I,2]←IF TIME>60 THEN TIME-10 ELSE 50;
IF I=JOINT THEN POSITION[I,1]←POSITION[I,2]←LAST_ARM[I]←LAST_ARM[I]+DTH
ELSE POSITION[I,1]←POSITION[I,2]←LAST_ARM[I];
END;
SCHEINMAN(DIAG,LAST_ARM);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
IFC BLUE THENC
COEFF[PTR2+6]←GO_WORD[JOINT]+2;
ELSEC
COEFF[PTR2+6]←GO_WORD[JOINT]+2;
ENDC
IF NNUL THEN COEFF[PTR2+6]←COEFF[PTR2+6]+'1000000;
ARRBLT(COEFF[PTR2-7],COEFF[PTR2],7);
KOE[1,1]←0;
ARRBLT(KOE[1,2],KOE[1,1],6*((MAX_SEG*3)+6)-1);
PACK_UP;
DURATION←10;
END;
ELSEC
SIMPLE MESSAGE PROCEDURE SET_TRANS(STRING S;REAL ARRAY T);
BEGIN "SET_TRANS"
INTEGER PTR;
STRING SS;
SAFE OWN REAL ARRAY E[1:6];
UNSTRUCT(T,E);
PTR←HASH(S);
WHILE LENGTH(SS←TRANSNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
IF ¬LENGTH(SS)
THEN BEGIN
IF FREE_DATA+2>FREE_DATA_LENGTH THEN RETURN;
TRANSNAM[PTR]←S;
TRANSNUM[PTR]←FREE_DATA;
FREE_DATA←FREE_DATA+2 END;
ARRBLT(DATA_BASE[TRANSNUM[PTR],1],E[1],6)
END "SET_TRANS";
SIMPLE MESSAGE PROCEDURE SET_VECT(STRING S;REAL ARRAY V);
BEGIN "SET_VECT"
INTEGER PTR;
STRING SS;
PTR←HASH(S);
WHILE LENGTH(SS←VECTNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
IF ¬LENGTH(SS)
THEN BEGIN
IF FREE_DATA+1>FREE_DATA_LENGTH THEN RETURN;
VECTNAM[PTR]←S;
VECTNUM[PTR]←FREE_DATA;
FREE_DATA←FREE_DATA+1 END;
ARRBLT(DATA_BASE[VECTNUM[PTR],1],V[1],3)
END "SET_VECT";
SIMPLE MESSAGE PROCEDURE GET_TRANS(STRING S;REAL ARRAY T);
BEGIN "GET_TRANS"
INTEGER PTR;
STRING SS;
SAFE OWN REAL ARRAY E[1:6];
PTR←HASH(S);
WHILE LENGTH(SS←TRANSNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
IF ¬LENGTH(SS) THEN RETURN;
ARRBLT(E[1],DATA_BASE[TRANSNUM[PTR],1],6);
CONSTRUCT(T,E)
END "GET_TRANS";
SIMPLE MESSAGE PROCEDURE GET_VECT(STRING S;REAL ARRAY V);
BEGIN "GET_VECT"
INTEGER PTR;
STRING SS;
PTR←HASH(S);
WHILE LENGTH(SS←VECTNAM[PTR]) ∧ ¬EQU(SS,S) DO PTR←REHASH;
IF ¬LENGTH(SS) THEN RETURN;
ARRBLT(V[1],DATA_BASE[VECTNUM[PTR],1],3);
V[4]←1.0
END "GET_VECT";
ENDC
PROCEDURE POLY (REAL ARRAY COEFFS; INTEGER #SEGS;
REAL ARRAY LOCS; INTEGER ARRAY TAU; REAL FST_VEL, LST_VEL, FST_ACC, LST_ACC;
INTEGER UNC1, UNC2);
BEGIN COMMENT: A trajectory is to be calculated. It is
composed of #SEGS segments, each to be a third degree poly-
nomial. The coefficients of the resulting polynomials will
fill COEFFS[1:N], where N is 4*#SEGS. At each point, 0 to
#SEGS, there is a given location in LOCS[0:#SEGS], except for points
UNC1 and UNC2, the unconstrained points. The velocities and
accelerations of the first and last points are also given:
these are FST_VEL, LST_VEL, FST_ACC, and LST_ACC.
The calculation depends on the array TAU[1:#SEGS], which
holds the time for each segment;
DEFINE MEM(ARG) "<>" = <MEMORY[ARG,REAL]>;
SAFE REAL ARRAY A [1:4*#SEGS,1:4*#SEGS];
SAFE REAL ARRAY B [1:4*#SEGS];
COMMENT: A is the large matrix and B is the vector.
We will solve Ax=B;
INTEGER ROW, COL, SEG, N, ALOC;
REAL TEMP;
ARRCLR(A);
ARRCLR(B);
N ← 4 * #SEGS;
UNC1 ← UNC1 + 1;
UNC2 ← UNC2 + 1;
ROW ← COL ← 1;
SEG ← 1;
COMMENT: Fill the matrix for the first segment;
ALOC ← LOCATION(A[1,1]);
A[ROW,COL] ← A[ROW+1,COL+1] ← 1.;
A[ROW+2,COL+2] ← 2.;
B[ROW] ← LOCS[0];
B[ROW+1] ← TAU[SEG] * FST_VEL;
B[ROW+2] ← TAU[SEG]↑2 * FST_ACC;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + 3*N + 4;
FOR SEG ← 2 STEP 1 UNTIL #SEGS DO
IF SEG=UNC1 ∨ SEG=UNC2
THEN BEGIN "UNCNST" COMMENT: Left of segment
is an unconstrained point;
MEM(ALOC) ← 1.;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ←
-1;
ALOC ← ALOC + N;
MEM(ALOC-3) ← -1;
MEM(ALOC-2) ← -2;
MEM(ALOC-1) ← -3;
MEM(ALOC+1) ← TEMP ← TAU[SEG-1]/TAU[SEG];
ALOC ← ALOC + N;
MEM(ALOC+2) ← TEMP*TEMP;
MEM(ALOC-1) ← -3;
MEM(ALOC-2) ← -1;
COMMENT: same effect as:
A[ROW,COL] ← 1.
A[ROW,COL-1] ← A[ROW,COL-2] ← A[ROW,COL-3]
← A[ROW,COL-4] ← A[ROW+1,COL-3]
← A[ROW+2,COL-2] ← -1.
A[ROW+1,COL-1] ← A[ROW+2,COL-1] ← -3.
A[ROW+1,COL-2] ← -2.
A[ROW+2,COL+2] ← (A[ROW+1,COL+1] ← TAU[SEG-1]
/TAU[SEG])↑2;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END
ELSE BEGIN "CNST" COMMENT: Left of segment is a
constrained point;
MEM(ALOC-1) ← MEM(ALOC-2) ← MEM(ALOC-3) ← MEM(ALOC-4)
← MEM(ALOC+N) ← 1.;
ALOC ← ALOC + N + N;
MEM(ALOC-1) ← -3;
MEM(ALOC-2) ← -2;
MEM(ALOC-3) ← -1;
MEM(ALOC+1) ← TEMP ← TAU[SEG-1] / TAU[SEG];
ALOC ← ALOC + N;
MEM(ALOC-1) ← -3;
MEM(ALOC-2) ← -1;
MEM(ALOC+2) ← TEMP*TEMP;
COMMENT: equivalent to:
A[ROW,COL-1] ← A[ROW,COL-2]
← A[ROW,COL-3] ← A[ROW,COL-4] ← A[ROW+1,COL] ← 1.
A[ROW+2,COL-3] ← A[ROW+3,COL-2] ← -1.
A[ROW+2,COL-1] ← A[ROW+3,COL-1] ← -3.
A[ROW+2,COL-2] ← -2.
A[ROW+3,COL+2] ← (A[ROW+2,COL+1] ← TAU[SEG-1] /
TAU[SEG])↑2;
B[ROW] ← B[ROW+1] ← LOCS[SEG-1];
ROW ← ROW + 4;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END;
COMMENT: Take care of the constraints at the final point;
COL ← COL - 4;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ← 1;
ALOC ← ALOC + N;
MEM(ALOC-3) ← 1.;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 3.;
ALOC ← ALOC + N;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 6.;
COMMENT: equivalent to:
A[ROW,COL] ← A[ROW,COL+1] ← A[ROW,COL+2] ← A[ROW,COL+3]
← A[ROW+1,COL+1] ← 1.
A[ROW+1,COL+2] ← A[ROW+2,COL+2] ← 2.
A[ROW+1,COL+3] ← 3.
A[ROW+2,COL+3] ← 6.;
B[ROW] ← LOCS[#SEGS];
B[ROW+1] ← TAU[#SEGS] * LST_VEL;
B[ROW+2] ← TAU[#SEGS]↑2 * LST_ACC;
ROW ← ROW + 3;
COL ← COL + 4;
IF ROW ≠ COL ∨ ROW ≠ N + 1 THEN OUTSTR("ERROR IN POLY"&CRLF);
DECOMPOSE(N,A,A);
SOLVE(N,A,B,COEFFS);
END;
PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5;
SAFE REAL ARRAY OSHOOT[1:6];
BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFF;REFERENCE REAL T,D);
RETURN( A4 ∧ OFF< (T←-(3*A3)/(4*A4)) <1+OFF ∧(D←(A4*T+A3)*T↑3/ABS(T))
∧ (IF D*DELTA>0 THEN (D←DELTA-D) ELSE (D←-D)));
IFC THROWING THENC REQUIRE "THROW.SAI" SOURCE_FILE; ENDC
INTEGER SIMPLE PROCEDURE KISEKI;BEGIN
INTEGER P2S,N,I,J;
INTEGER FREE1,FREE2;
REAL LOV,SDV,ANG,T;
SIMPLE PROCEDURE BUMP_UP(INTEGER I;REAL ANG,T);
BEGIN
INTEGER J;
FOR J←MAX_SEG STEP -1 UNTIL I+1 DO
BEGIN
POSITION[JOINT,J]←POSITION[JOINT,J-1];
PERIOD[JOINT,J]←PERIOD[JOINT,J-1]
END;
POSITION[JOINT,I]←ANG;
PERIOD[JOINT,I]←T*PERIOD[JOINT,I];
IF PERIOD[JOINT,I]<1 THEN PERIOD[JOINT,I]←1;
PERIOD[JOINT,I+1]←PERIOD[JOINT,I+1]-PERIOD[JOINT,I];
IF I≤FREE1 THEN FREE1←FREE1+1;
IF I≤FREE2 THEN FREE2←FREE2+1;
END;
P2S←PTR2-7;
FOR JOINT←1 STEP 1 UNTIL 6 DO
BEGIN
COMMENT FREE1 AND FREE2 POINT TO UNCONSTRAINED TRAJ POINTS;
FREE1←2;
FREE2←3;
COMMENT NUMBER OF SEGMENTS;
N←NS[JOINT];
COMMENT LiftOffVelocity ans SetDownVelocity;
LOV←2*(POSITION[JOINT,1]-POSITION[JOINT,0]);
SDV←2*(POSITION[JOINT,5]-POSITION[JOINT,4]);
ARRBLT(POS[0],POSITION[JOINT,1],4);
ARRBLT(TIM[1],PERIOD[JOINT,2],3);
POLY(SOLN,3,POS,TIM,LOV/PERIOD[JOINT,1],SDV/PERIOD[JOINT,5],0,0,1,2);
COMMENT CHECK FOR OVERSHOOT;
FOR I←4 STEP -1 UNTIL 2 DO
BEGIN
J←(I*4)-7;
IF QUADROOT(SOLN[J],SOLN[J+1],
SOLN[J+2],SOLN[J+3],ANG,T)
THEN BEGIN
BUMP_UP(I,ANG,T);
N←NS[JOINT]←NS[JOINT]+1;
FOR J←FREE1,FREE2 DO
BEGIN
I←PERIOD[JOINT,J]+PERIOD[JOINT,J+1];
PERIOD[JOINT,J+1]←I/2;
PERIOD[JOINT,J]←I-PERIOD[JOINT,J+1];
END;
ARRBLT(POS[0],POSITION[JOINT,1],N-1);
ARRBLT(TIM[1],PERIOD[JOINT,2],N-2);
POLY(SOLN,N-2,POS,TIM,LOV/PERIOD[JOINT,1],SDV/PERIOD[JOINT,N],
0,0,FREE1-1,FREE2-1);
DONE;
END;
END;
POSITION[JOINT,FREE1]←SOLN[FREE1*4-3];
POSITION[JOINT,FREE2]←SOLN[FREE2*4-3];
ARRBLT(KOE[JOINT,6],SOLN[1],(N-2)*4);
KOE[JOINT,1]←POSITION[JOINT,0];
KOE[JOINT,2]←KOE[JOINT,3]←KOE[JOINT,N*4]←0;
KOE[JOINT,4]←LOV;
KOE[JOINT,5]←-LOV/2;
KOE[JOINT,N*4-2]←POSITION[JOINT,N-1];
KOE[JOINT,N*4-1]←SDV;
KOE[JOINT,N*4+1]←-SDV;
KOE[JOINT,N*4+2]←SDV/2;
END;
FOR I←1 STEP 1 UNTIL N←NS[6] DO
BEGIN
FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,0.5+(NS[J]*I)/N];
SCHEINMAN(DIAG,TH);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
END;
PTR2←P2S;
PACK_UP;
RETURN(0) END;
PROCEDURE TRAJECTORY(REAL ARRAY T0,TF,TRANS);
BEGIN
INTEGER MT,I,J,N;
REAL INT,R;
SAFE REAL ARRAY AI,AF,DTH[1:6];
SAFE REAL ARRAY DV[1:4];
INTEGER TR1,TR2;
PROCEDURE LIFTOFF(REAL ARRAY T0,T,DEP,DTH; REFERENCE INTEGER TR);
BEGIN REAL ARRAY TT[1:4,1:4];
INTEGER I,J;
REAL R;
ARRTRAN(DTH,T0);
ARRTRAN(TT,T);
FOR I←1 STEP 1 UNTIL 3 DO TT[I,4]←T[I,4]+DEP[I];
ARM_SOLVE(TT,DTH);
FOR I←1 STEP 1 UNTIL 6 DO
BEGIN
DTH[I]←DTH[I]-T0[I];
R←2*DTH[I]+T0[I];
IF (STOP[I,1]-R)*(R-STOP[I,2])<0.0
THEN BEGIN
FOR J←1,2 DO
IF (R-STOP[I,J])*(STOP[I,J]-T0[I])≥0
THEN BEGIN
DTH[I]←DTH[I]*.9*(STOP[I,J]-T0[I])/(R-T0[I]);
DONE;
END;
END;
END;
TR←IF T1_ARM>0 THEN T1_ARM ELSE RUNTIME(DTH);
IF T1_ARM ≤0 THEN TR←IF (R←15*SQRT(MAGNITUDE(DEP)))>TR THEN R ELSE TR;
END;
FLUSHP(150,T0);
LIFTOFF(T0,LAST_TRANS,DEPART_ARM,AI,TR1);
LIFTOFF(TF,TRANS,ARRIVE_ARM,AF,TR2);
FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←TF[I]+AF[I]-T0[I]-AI[I];
PLUS(DV,ARRIVE_ARM,DEPART_ARM);
R←10*SQRT(MAGNITUDE(DV));
COMMENT MINIMUN TIME TO ACCOMPLISH CHANGE IN VELOCITY;
N←IF T2_ARM>0 THEN T2_ARM ELSE RUNTIME(DTH)+R;
FOR I←1 STEP 1 UNTIL 6 DO BEGIN
POSITION[I,0]←T0[I];
POSITION[I,1]←T0[I]+AI[I];
POSITION[I,4]←TF[I]+AF[I];
POSITION[I,5]←TF[I];
NS[I]←5;
PERIOD[I,1]←TR1;
PERIOD[I,2]←PERIOD[I,3]←N/3;
PERIOD[I,4]←N-PERIOD[I,2]-PERIOD[I,3];
PERIOD[I,5]←TR2 END;
LOOP←0;
DURATION←N;
IF KISEKI THEN USERERR(0,1,"OVERSHOOT");
ARRTRAN(LAST_ARM,TF);
ARRTRAN(LAST_PLANNED_ARM,TF);
ARM_TIME←ARM_TIME+N+TR1*2;
ARRTRAN(DEPART_ARM,ARRIVE_ARM);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
T1_ARM←T2_ARM←0;
RESET_CONO;
END;
BOOLEAN SIMPLE PROCEDURE IS_NOT_CLEAR(SAFE REAL ARRAY TRANS,J);
BEGIN SAFE OWN REAL ARRAY IP[1:4];
REAL R;
BOOLEAN SIMPLE PROCEDURE NOT_CLEAR(REFERENCE REAL J;SAFE REAL ARRAY V);
BEGIN SAFE OWN REAL ARRAY VT[1:4];
EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY V;REFERENCE REAL T;REAL ARRAY F);
TRANSFORM(VT,J,V);
IF VT[3]<0.375 THEN RETURN(TRUE);
IF (VT[1]-SHOLDER[1])↑2+(VT[2]-SHOLDER[2])↑2<25.0 THEN RETURN(TRUE) ELSE RETURN(FALSE);
END;
IP[1]←IP[3]←0.0;IP[4]←1.0;
FOR R←-2.0,2.0 DO BEGIN IP[2]←R;IF NOT_CLEAR(TRANS[1,1],IP)THEN RETURN(TRUE);END;
ARRTRAN(J,LAST_ARM);
J[4]←-90.0;J[6]←0;
IF PSOLVE(TRANS,J) THEN RETURN (TRUE);
HANDPOS(J);
IP[2]←0.0;
FOR R←3.0,-3.0 DO BEGIN IP[3]←R;IF NOT_CLEAR(T[SQAR(4)],IP) THEN RETURN(TRUE);END;
IP[3]←-38.0;
IF NOT_CLEAR(T[SQAR(3)],IP) THEN RETURN(TRUE) ELSE RETURN(FALSE);
END;
SIMPLE MESSAGE PROCEDURE TO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
BEGIN SAFE OWN REAL ARRAY J[1:6];
SAFE OWN REAL ARRAY EXF[1:7];
INTEGER K;
FLAG←0;
IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
FLAG←-1;
IF ARM_EXECUTE THEN BEGIN
ARM_MESSAGE[21]←'17000000;
ARRBLT(ARM_MESSAGE[1],T[1,1],12);
SCHEINMAN(EXF,J);
ARRBLT(ARM_MESSAGE[14],EXF[1],7);
ARM_MESSAGE[13]←T2_ARM;
RESET_CONO;
ARMFN(20);
ARM_EXECUTE←FALSE;
UPDATE_SEG;
RETURN END;
FLUSHP(30,LAST_ARM);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'417000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'17000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],T[1,1],12);
COEFF[PTR4←PTR4+12]←T2_ARM;
SCHEINMAN(DIAG,J);
ARRBLT(COEFF[PTR4←PTR4+1],DIAG[1],7);
PTR4←PTR4+6;
ARRTRAN(LAST_ARM,J);
ARM_TIME←ARM_TIME+T2_ARM;
ARRTRAN(LAST_PLANNED_ARM,J);
ARRTRAN(LAST_PLANNED_TRANS,T);
ARRTRAN(LAST_TRANS,T);
ARRTRAN(DEPART_ARM,ARRIVE_ARM);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
T1_ARM←T2_ARM←0;
RESET_CONO;
END;
SIMPLE MESSAGE PROCEDURE MOVE_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
BEGIN SAFE OWN REAL ARRAY J[1:6];
IF ARM_EXECUTE THEN BEGIN TO_ARM(T,FLAG); RETURN END;
FLAG←0;
IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
FLAG←-1;
TRAJECTORY(LAST_ARM,J,T);
ARRTRAN(LAST_TRANS,T);
ARRTRAN(LAST_PLANNED_TRANS,T);
END;
SIMPLE MESSAGE PROCEDURE GO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
BEGIN SAFE OWN REAL ARRAY TF[1:6];
INTEGER K,TIME,MT,I,J,NNULS;
REAL INT,R;
SAFE OWN REAL ARRAY TH,DTH,AI,AF[1:6];
SAFE OWN REAL ARRAY T1,T2[1:4,1:4];
INTEGER TR1,TR2;
SIMPLE PROCEDURE LIFTOFF(REAL ARRAY T0,T,DEP,DTH; REFERENCE INTEGER TR);
BEGIN REAL OWN ARRAY TT[1:4,1:4];
INTEGER I,J;
REAL R;
ARRTRAN(DTH,T0);
ARRTRAN(TT,T);
FOR I←1 STEP 1 UNTIL 3 DO TT[I,4]←T[I,4]+DEP[I];
ARM_SOLVE(TT,DTH);
FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←DTH[I]-T0[I];
TR←IF T1_ARM>0 THEN T1_ARM ELSE RUNTIME(DTH);
IF T1_ARM ≤0 THEN TR←IF (R←15*SQRT(MAGNITUDE(DEP)))>TR THEN R ELSE TR;
END;
IF ARM_EXECUTE THEN BEGIN TO_ARM(T,FLAG); RETURN END;
FLAG←0;
IF IS_NOT_CLEAR(T,TF) THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
FLAG←-1;
FLUSHP(90,LAST_ARM);
LIFTOFF(LAST_ARM,LAST_TRANS,DEPART_ARM,AI,TR1);
LIFTOFF(TF,T,ARRIVE_ARM,AF,TR2);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'420000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'20000000+(PTR4←PTR4+1);
INVERT(T1,LAST_TRANS);
MOVET(T2,LAST_TRANS);
FOR I←1 STEP 1 UNTIL 3 DO T2[I,4]←T2[I,4]+DEPART_ARM[I];
TIMES(T1,T1,T2);
ARRBLT(COEFF[PTR4],T1[1,1],12);
COEFF[PTR4←PTR4+12]←TR1;
INVERT(T1,T);
MOVET(T2,T);
FOR I←1 STEP 1 UNTIL 3 DO T2[I,4]←T2[I,4]+ARRIVE_ARM[I];
TIMES(T1,T1,T2);
ARRBLT(COEFF[PTR4←PTR4+1],T1[1,1],12);
COEFF[PTR4←PTR4+12]←T2_ARM;
ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
COEFF[PTR4←PTR4+12]←TR2;
FOR I←1 STEP 1 UNTIL 6 DO TH[I]←LAST_ARM[I]+AI[I];
SCHEINMAN(DIAG,TH);
ARRBLT(COEFF[PTR4←PTR4+1],DIAG[1],7);
FOR I←1 STEP 1 UNTIL 6 DO TH[I]←TF[I]+AF[I];
SCHEINMAN(DIAG,TH);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR4←PTR4+7],DIAG[1],7);
SCHEINMAN(DIAG,TF);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR4←PTR4+7],DIAG[1],7);
PTR4←PTR4+6;
ARRTRAN(LAST_ARM,TF);
ARM_TIME←ARM_TIME+T2_ARM+TR1+TR2;
ARRTRAN(LAST_PLANNED_ARM,TF);
ARRTRAN(LAST_PLANNED_TRANS,T);
ARRTRAN(LAST_TRANS,T);
ARRTRAN(DEPART_ARM,ARRIVE_ARM);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
T1_ARM←T2_ARM←0;
RESET_CONO;
END;
SIMPLE MESSAGE PROCEDURE DRAW_ARM(REAL ARRAY PROFILE;INTEGER STAT);
BEGIN
SAFE OWN REAL ARRAY DTH[1:6];
SAFE OWN REAL ARRAY FAS[0:6,1:6];
SAFE OWN REAL ARRAY ROTS,ROTA,RS,IP,RV,CV,FVV,FV,VT,DIFF[1:4];
REAL AR,RR,THP4,R,MAX;
INTEGER TIME,NF,I,J,K,NT;
EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY R;REFERENCE REAL A);
IF ARM_EXECUTE THEN BEGIN STAT←4;ARM_EXECUTE←FALSE;RETURN END;
ARRTRAN(TRANS,LAST_TRANS);
CVV(IP,LAST_TRANS,4);
ARRTRAN(FAS,FREE_ARM);
MOVEV(DIFF,PROFILE[1,1]);
MOVEV(RV,PROFILE[2,1]);
IF(AR←IF MAGNITUDE(RV) THEN PROFILE[3,1] ELSE 0) THEN UNIT(RV,RV);
MOVEV(ROTA,PROFILE[5,1]);
IF(RR←IF MAGNITUDE(ROTA) THEN PROFILE[3,2] ELSE 0)
THEN BEGIN UNIT(ROTA,ROTA);
MOVEV(ROTS,PROFILE[4,1]);
DIFFERENCE(IP,IP,ROTS) END;
FVV[1]←FV[1]←FORCE_ARM[1];
FVV[2]←FV[2]←FORCE_ARM[2];
FVV[3]←FV[3]←FORCE_ARM[3];
FVV[4]←FV[4]←1.0;
FLUSH(0,LAST_ARM);
SCHEINMAN(DIAG,LAST_ARM);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
FRST_OPEN←TRUE;
R←0;
IF PROFILE[0,3] THEN LOOP←PROFILE[0,3]-1;
IF LOOP THEN NT←8 ELSE BEGIN
NT←4;
R←MAGNITUDE(DIFF);
IF R>15.0 THEN NT←(R/5)+0.5;
NT←IF (MAX←AR/60.0)>NT THEN MAX ELSE NT;
NT←IF (MAX←RR/60.0)>NT THEN MAX ELSE NT END;
IF TYP_HAND THEN OUTSTR(CVS(NT)&" PART TRAJECTORY"&CRLF);
IF NT>8 THEN BEGIN STAT←3;RETURN END;
FOR J←1 STEP 1 UNTIL 6 DO TH[I]←POSITION[J,0]←LAST_ARM[J];
FOR I←1 STEP 1 UNTIL NT DO
BEGIN
R←IF I=1
THEN 1/(3*(NT-2))
ELSE (I-1)/(NT-2);
R←IF I=NT-1
THEN R-1/(3*(NT-2))
ELSE IF I=NT
THEN 1.0
ELSE R;
SCALE(VT,DIFF,R);
PLUS(VT,VT,IP);
IF RR THEN BEGIN ROTATE(RS,ROTS,ROTA,R*RR);
PLUS(VT,VT,RS)END;
REDUCE(VT);
CVC(TRANS,4,VT);
IF AR THEN FOR J←1 STEP 1 UNTIL 3 DO BEGIN
CVV(CV,LAST_TRANS,J);
REVOLVE(CV,RV,R*AR);
CVC(TRANS,J,CV) END;
IF (STAT←ARM_SOLVE(TRANS,TH)) THEN RETURN;
FOR J←1 STEP 1 UNTIL 6 DO BEGIN
POSITION[J,I]←TH[J];
DTH[I]←POSITION[J,I]-POSITION[J,I-1] END;
IF (K←RUNTIME(DTH))*NT > PROFILE[0,2] THEN PROFILE[0,2]←K*NT;
IF RR THEN BEGIN
FOR K←1 STEP 1 UNTIL FAS[0,1] DO
BEGIN
MOVEV(VT,FAS[K,1]);
VT[4]←1.0;
IF MAGNITUDE(VT)>0.0 THEN
BEGIN
REVOLVE(VT,ROTA,R*RR);
REDUCE(VT);
FOR J←1 STEP 1 UNTIL 3 DO FREE_ARM[K,J]←VT[J] END END;
MOVEV(FV,FVV[1]);
REVOLVE(FV,ROTA,R*RR);
REDUCE(FV);
ARRBLT(FORCE_ARM[1],FV[1],3) END;
SCHEINMAN(DIAG,TH);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
END;
TIME←(PROFILE[0,2]/NT)+0.5;
FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
NS[JOINT]←NT;
FOR J←1 STEP 1 UNTIL NT DO PERIOD[JOINT,J]←TIME;
ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
OUTSTR("DRAW DOSNT WORK");
COMMENT POLY(NS[JOINT]);
ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+6) END;
PTR2←486;
PACK_UP;
DURATION←TIME;
ARRTRAN(LAST_ARM,TH);
ARRTRAN(LAST_TRANS,TRANS);
ARRTRAN(LAST_PLANNED_ARM,TH);
ARRTRAN(LAST_PLANNED_TRANS,TRANS);
ARM_TIME←ARM_TIME+TIME*((NT-2)*LOOP+2);
STAT←0;
RESET_CONO;
FRST_OPEN←FALSE;
END;
SIMPLE MESSAGE PROCEDURE START_TRAJECTORY(STRING FILE;INTEGER START);
BEGIN
STRING S;
LABEL FIND,NONE,FOUND;
REAL R;
INTEGER I;
FRST_OPEN←TRUE;
ARM_TIME←0;
IF ¬START THEN ARRTRAN(LAST_ARM,ARM_VECTOR);
ARRTRAN(LAST_PLANNED_ARM,LAST_ARM);
HANDPOS(LAST_ARM);
ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
ARRTRAN(LAST_PLANNED_TRANS,LAST_TRANS);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←DEPART_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
RESET_CONO;
IF FAST THEN BEGIN
FIND: IF NEXT_BAND>'37 THEN GO TO NONE;
FOR BAND←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[BAND])THEN GO TO FOUND;
TRACK[BAND]←CALL('100000000000 LOR BAND,"UFBGET");
IF ¬_SKIP_ THEN GO TO NONE;
NEXT_BAND←BAND+1;
BANDS[BAND]←FILE;
FOUND: GOODIE[3]←0;
BAND_NUMBER←TRACK[BAND];
END ELSE NONE:BEGIN
FAST←FALSE;
OPEN('16,"DSK",'17,0,0,120,BREAK,EOF);
ENTER('16,FILE&".TRJ",BREAK);
END;
COEFF[0]←0; ARRBLT(COEFF[1],COEFF[0],'1037);
PTR4←PTR3←0;
PTR2←512;
END;
SIMPLE MESSAGE PROCEDURE CLOSE_TRAJECTORY;
FLUSH(1,TH);
SIMPLE MESSAGE PROCEDURE ARM_POSITION(STRING SAVE_CELL);
BEGIN
IF LENGTH(SAVE_CELL)
THEN BEGIN
TRANS_ARM[4,1]←TRANS_ARM[4,2]←TRANS_ARM[4,3]←0;
TRANS_ARM[4,4]←1
END;
ARMPOS(CVSIX(SAVE_CELL));
UPDATE_SEG;
END;
SIMPLE MESSAGE PROCEDURE OPEN_HAND(REAL OPENING);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'1000000 LOR (OPENING LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN
FLUSHP(150,LAST_ARM);
STACK[PTR3←PTR3+1]←'1000000 LOR (OPENING LSH -18) END;
SIMPLE MESSAGE PROCEDURE WOBBLE_HAND(REAL WOBBLEING);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'14000000 LOR (WOBBLEING LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(1,LAST_ARM);STACK[PTR3←PTR3+1]←'14000000 LOR (WOBBLEING LSH -18) END;
SIMPLE MESSAGE PROCEDURE PLACE_ARM;
BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
INTEGER I;
FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=3 THEN -0.03 ELSE 0;
DIR[4]←1.0;
INCREMENT(DTH,DIR,FALSE);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[7]←'4000000;
ARRBLT(ARM_MESSAGE[1],DTH[1],6);
ARMFN(6);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(7,LAST_ARM);
STACK[PTR3←PTR3+1]←'4000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],DTH[1],6);
PTR4←PTR4+5 END;
END;
SIMPLE MESSAGE PROCEDURE SEARCH_ARM(REAL SIZE;REAL ARRAY N,F);
BEGIN SAFE OWN REAL ARRAY SEC[1:4];
IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
CROSS(SEC,N,F);
UNIT(F,F);
UNIT(SEC,SEC);
SCALE(F,F,SIZE);
SCALE(SEC,SEC,SIZE);
REDUCE(F);
REDUCE(SEC);
FLUSHP(40,LAST_ARM);
STACK[PTR3←PTR3+1]←'15000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],F[1],3);
ARRBLT(COEFF[PTR4←PTR4+3],SEC[1],3);
ARRBLT(COEFF[PTR4←PTR4+3],LAST_PLANNED_TRANS[1,1],12);
ARRBLT(COEFF[PTR4←PTR4+12],LAST_PLANNED_ARM[1],6);
PTR4←PTR4+6;
END;
SIMPLE MESSAGE PROCEDURE PARK_ARM;
BEGIN INTEGER K;
IF ¬ARM_EXECUTE THEN
BEGIN
ARRIVE_ARM[3]←0.0;
GO_ARM(PARK_TRANS,K);
END ELSE TO_ARM(PARK_TRANS,K);
END;
SIMPLE MESSAGE PROCEDURE WAIT_ARM(STRING S);
IF ¬ARM_EXECUTE THEN BEGIN
FLUSHP(16,LAST_ARM);
STACK[PTR3←PTR3+1]←'3000000+(PTR4←PTR4+1);
PTR4←PTR4+PSTRING(COEFF[PTR4],S[1 FOR 74]);
END ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE CLOSE_HAND(REAL DIST);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'2000000 LOR (DIST LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(150,LAST_ARM);STACK[PTR3←PTR3+1]←'2000000 LOR (DIST LSH -18)END;
SIMPLE MESSAGE PROCEDURE CENTER_HAND(REAL DIST);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'12000000 LOR (DIST LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(150,LAST_ARM);STACK[PTR3←PTR3+1]←'12000000 LOR (DIST LSH -18)END;
SIMPLE MESSAGE PROCEDURE CHANGE_ARM(REAL ARRAY DIR;REAL DIST;
REAL ARRAY AXIS;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
BEGIN INTEGER J,N;
SAFE OWN REAL ARRAY AV[1:6];
SAFE OWN REAL ARRAY EXF[1:7];
SAFE OWN REAL ARRAY DT,TT[1:4,1:4];
SAFE OWN REAL ARRAY VT[1:4];
IF ARM_EXECUTE
THEN ARRBLT(TT[1,1],ARM_LINK[6,1,1],16)
ELSE ARRTRAN(TT,LAST_TRANS);
SCALE(VT,DIR,DIST);
REDUCE(VT);
ARRTRAN(DT,TT);
FOR J←1 STEP 1 UNTIL 3 DO DT[J,4]←DT[J,4]+VT[J];
IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
UNIT(AXIS,AXIS);
FOR I←1 STEP 1 UNTIL 3 DO BEGIN
CVV(VT,DT,I);
REVOLVE(VT,AXIS,DEG);
CVC(DT,I,VT) END;
END;
INVERT(TT,TT);
TIMES(TT,TT,DT);
IF ARM_EXECUTE THEN ARRTRAN(AV,ARM_VECTOR) ELSE ARRTRAN(AV,LAST_ARM);
FLAG←¬ARM_SOLVE(DT,AV);
IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[21]←'5000000;
ARRBLT(ARM_MESSAGE[1],TT[1,1],12) END
ELSE BEGIN ARRTRAN(LAST_ARM,AV);
FLUSHP(70,AV);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'405000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'5000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],TT[1,1],12) END;
SCHEINMAN(EXF,AV);
IF ARM_EXECUTE
THEN ARRBLT(ARM_MESSAGE[13],EXF[1],7)
ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+12],EXF[1],7);
PTR4←PTR4+6 END;
RESET_CONO;
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[20]←TIME;
ARMFN(20);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN COEFF[PTR4←PTR4+1]←TIME;
ARM_TIME←ARM_TIME+TIME;
ARRTRAN(LAST_TRANS,DT) END;
END;
SIMPLE MESSAGE PROCEDURE SET_ARM(STRING ST;REAL ARRAY V,WRT);
BEGIN
SAFE OWN REAL ARRAY T[1:4,1:4];
INVERT(T,WRT);
TIMES(T,V,T);
IF ARM_EXECUTE THEN BEGIN
ARM_MESSAGE[14]←'13000000;
ARM_MESSAGE[1]←CVSIX(ST);
ARRBLT(ARM_MESSAGE[2],T[1,1],12);
ARMFN(13);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN
FLUSHP(24,LAST_ARM);
STACK[PTR3←PTR3+1]←'13000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(ST);
ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
PTR4←PTR4+11 END;
END"SET_ARM";
SIMPLE MESSAGE PROCEDURE MOVING(STRING ST;REAL ARRAY V);
BEGIN
IF ARM_EXECUTE THEN BEGIN
ARM_MESSAGE[5]←'24000000;
ARM_MESSAGE[1]←CVSIX(ST);
ARRBLT(ARM_MESSAGE[2],V[1],3);
ARMFN(3);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN
FLUSHP(4,LAST_ARM);
STACK[PTR3←PTR3+1]←'24000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(ST);
ARRBLT(COEFF[PTR4←PTR4+1],V[1],3);
PTR4←PTR4+2 END;
END"MOVING";
SIMPLE MESSAGE PROCEDURE DRIVE_ARM(INTEGER JOINT;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
BEGIN INTEGER I,J,N;
REAL R;
SAFE OWN REAL ARRAY EXF[1:7];
SAFE OWN REAL ARRAY LA[1:6];
FLAG←FALSE;
IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
R←LA[JOINT]+DEG;
IF (STOP[JOINT,1]-R)*(R-STOP[JOINT,2])<1 THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
FLAG←TRUE;
LA[JOINT]←R;
SCHEINMAN(EXF,LA);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[15]←'23000000;
ARM_MESSAGE[1]←0;ARRBLT(ARM_MESSAGE[2],ARM_MESSAGE[1],5);
ARRBLT(ARM_MESSAGE[JOINT],DEG,1) END
ELSE BEGIN ARRBLT(LAST_TRANS[1,1],T[85],16);
FLUSHP(50,LAST_ARM);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'423000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'23000000+(PTR4←PTR4+1);
COEFF[PTR4]←0;
ARRBLT(COEFF[PTR4+1],COEFF[PTR4],5);
ARRBLT(COEFF[PTR4+JOINT-1],DEG,1) END;
IF ARM_EXECUTE
THEN BEGIN ARRBLT(ARM_MESSAGE[7],EXF[1],7);
ARM_MESSAGE[13]←GO_WORD[JOINT]+2;
IF FREE_ARM[0,1] THEN FREE_JOINT(ARM_MESSAGE[13]);
ARM_MESSAGE[14]←TIME;
ARMFN(14);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+6],EXF[1],7);
PTR4←PTR4+6;
IFC BLUE THENC
COEFF[PTR4]←GO_WORD[JOINT]+4;
ELSEC
COEFF[PTR4]←GO_WORD[JOINT]+2;
ENDC
IF NNUL THEN COEFF[PTR4]←COEFF[PTR4]+'1000000;
IF FREE_ARM[0,1] THEN FREE_JOINT(COEFF[PTR4]);
COEFF[PTR4←PTR4+1]←TIME;
ARM_TIME←ARM_TIME+TIME;
ARRTRAN(LAST_ARM,LA) END;
RESET_CONO;
END;
SIMPLE MESSAGE PROCEDURE STOP_ARM(REAL ARRAY F,M);
BEGIN SAFE OWN REAL ARRAY TQ,XF[1:6];
INTEGER I;
REAL FAC,R,MAX;
HANDPOS(LAST_ARM);
REDUCE(F);
ARRBLT(XF[1],F[1],3);
REDUCE(M);
ARRBLT(XF[4],M[1],3);
FORCE(TQ,XF);
MAX←0.0;
FAC←1.5;
FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(TQ[I]/F0[I]))>MAX THEN BEGIN
MAX←R;
BFJ←I END;
R←FAC/MAX;
IF R>1.0 THEN FOR I←1 STEP 1 UNTIL 6 DO TQ[I]←TQ[I]*R;
FLUSHP(7,LAST_ARM);
STACK[PTR3←PTR3+1]←'7000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],TQ[1],6);
PTR4←PTR4+5;
END;
SIMPLE MESSAGE PROCEDURE NO_NULL;NNUL←-1;
SIMPLE MESSAGE PROCEDURE TALK_ARM;INTERP←TRUE;
SIMPLE MESSAGE PROCEDURE DO_IT(INTEGER PPPN;STRING FILE);
BEGIN INTEGER I;
ARM_STATUS←0;
ARM_EXECUTE←FALSE;
FOR I←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[I])THEN DONE;
DOIT(PPPN,TRACK[I],CVSIX(FILE));
IF ARM_STATUS='50 ∧TRACK[I] THEN BANDS[I]←NULL;
UPDATE_SEG;
END;
SIMPLE MESSAGE PROCEDURE MERGE_ARM;MERGE←TRUE;
SIMPLE MESSAGE PROCEDURE DO_PROCEED(INTEGER SKIP);
BEGIN
ARM_EXECUTE←FALSE;
IF ¬ARM_WAIT THEN RETURN;
ARMPROCEED(SKIP);
UPDATE_SEG;
END;
SIMPLE MESSAGE PROCEDURE OTHER_GO;
IF ARM_EXECUTE
THEN BEGIN
ARM_MESSAGE[1]←'27000000;
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG
END
ELSE BEGIN
FLUSHP(1,LAST_ARM);
STACK[PTR3←PTR3+1]←'27000000
END;
SIMPLE MESSAGE PROCEDURE OTHER_RUN;
IF ARM_EXECUTE
THEN BEGIN
ARM_MESSAGE[1]←'31000000;
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG
END
ELSE BEGIN
FLUSHP(1,LAST_ARM);
STACK[PTR3←PTR3+1]←'31000000
END;
SIMPLE MESSAGE PROCEDURE OTHER_WAIT;
IF ARM_EXECUTE
THEN BEGIN
ARM_MESSAGE[1]←'30000000;
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG
END
ELSE BEGIN
FLUSHP(1,LAST_ARM);
STACK[PTR3←PTR3+1]←'30000000
END;
SIMPLE MESSAGE PROCEDURE ARM_CONO(REAL ARRAY APPROACH,OBJECT;INTEGER DEPROACH_TIME,MID_TIME);
BEGIN ARRTRAN(ARRIVE_ARM,APPROACH);
FLUSHP(4,LAST_ARM);
OBJECT_MASS←OBJECT[4];
OBJECT_KXX←OBJECT[1];
OBJECT_KYY←OBJECT[2];
OBJECT_KZZ←OBJECT[3];
T2_ARM←MID_TIME;
T1_ARM←DEPROACH_TIME;
END;
SIMPLE MESSAGE PROCEDURE SET_TOUCH(BOOLEAN STOP_ON_TOUCH);
IF ¬ARM_EXECUTE THEN BEGIN FLUSHP(1,LAST_ARM);
STACK[PTR3←PTR3+1]←'6000000;
IF STOP_ON_TOUCH THEN STACK[PTR3]←STACK[PTR3] LOR '777777;
END ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SKIPE(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'101000000 LOR I ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SKIPN(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'103000000 LOR I ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SKIPS(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'104000000 LOR I ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_JMP(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'102000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_AOJ(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'16000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SOJG(STRING S;INTEGER I);
IF ARM_EXECUTE THEN BEGIN
ARM_EXECUTE←FALSE;;
RETURN END ELSE BEGIN
FLUSHP(3,LAST_ARM);
STACK[PTR3←PTR3+1]←'26000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(S);
COEFF[PTR4←PTR4+1]←I END;
SIMPLE MESSAGE PROCEDURE ARM_ASSIGN(STRING S;INTEGER VAL);
IF ARM_EXECUTE THEN BEGIN
ARM_MESSAGE[3]←'25000000;
ARM_MESSAGE[1]←CVSIX(S);
ARM_MESSAGE[2]←VAL;
ARMFN(2);
UPDATE_SEG END ELSE
BEGIN
FLUSHP(3,LAST_ARM);
STACK[PTR3←PTR3+1]←'25000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(S);
COEFF[PTR4←PTR4+1]←VAL END;
SIMPLE MESSAGE PROCEDURE ARM_SAVE(STRING S);
BEGIN
SAFE OWN REAL ARRAY T[1:4,1:4];
INVERT(T,LAST_PLANNED_TRANS);
IF ARM_EXECUTE THEN BEGIN ARM_MESSAGE[13]←'10000000;
ARM_MESSAGE[1]←CVSIX(S) END ELSE
BEGIN FLUSHP(30,LAST_ARM);STACK[PTR3←PTR3+1]←'10000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(S) END;
IF ARM_EXECUTE THEN ARRBLT(ARM_MESSAGE[2],T[1,1],12)
ELSE ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
IF ARM_EXECUTE THEN BEGIN ARMFN(13);
ARM_EXECUTE←FALSE;
UPDATE_SEG END ELSE PTR4←PTR4+12;
END;
SIMPLE MESSAGE PROCEDURE ARM_RESTORE(STRING S;INTEGER FINISH);
BEGIN
LABEL L1;
INTEGER I,J;
SAFE OWN REAL ARRAY T[1:4,1:4];
IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
FOR I←PTR3 STEP -1 UNTIL 1 DO BEGIN
J←STACK[I] LAND '77000000;
IF '17000000 ≤ J ≤'21000000 THEN BEGIN
FOR J←PTR3 STEP -1 UNTIL I DO BEGIN
STACK[J+1]←STACK[J];
END;
PTR3←PTR3+1;
GO TO L1
END;
END;
RETURN;
L1: STACK[I]←'11000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(S);
COEFF[PTR4←PTR4+1]←FINISH;
ARRBLT(COEFF[PTR4←PTR4+1],LAST_PLANNED_TRANS[1,1],12);
ARRBLT(COEFF[PTR4←PTR4+12],LAST_PLANNED_ARM[1],6);
PTR4←PTR4+6;
END;
SIMPLE MESSAGE PROCEDURE SCREW(REAL VELOCITY);
BEGIN INTEGER I;
IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
STACK[PTR3←PTR3+1]←'22000000 LOR ((I←VELOCITY*1.0) LAND '777777) END;